#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/transport/transport.hh>
#include <gazebo/common/common.hh>
#include <ignition/math/Vector3.hh>

namespace gazebo {

static const ignition::math::Vector3d defaultforce(0.0, 0.0, 0.0);
static constexpr double defaultduation{0.0};
static const std::string defaultlink = "base_link";

class AirLaunchPlugin : public ModelPlugin {
private:
    physics::ModelPtr model;             
    physics::LinkPtr link;                                 
    event::ConnectionPtr update_connection;

    std::string link_name{defaultlink};  
    ignition::math::Vector3d force{defaultforce};
    double duration{defaultduation};
    double _duration{0.0}; 

    transport::NodePtr node;
    transport::SubscriberPtr sub;

    gazebo::common::Time trigger_time;

    bool isLaunched = false;

public:

    void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) override {

        model = _model;

        if (_sdf->HasElement("linkName")) {
            link_name = _sdf->GetElement("linkName")->Get<std::string>();
            link = model->GetLink(link_name);
        } else {
            gzerr << "[airlaunch_plugin] Please specify a linkName of the object.\n";
        }


        if (_sdf->HasElement("force")) {
            this->force = _sdf->Get<ignition::math::Vector3d>("force");
        } else {
            gzerr << "[airlaunch_plugin] Please specify the force apply to the object.\n";
        }

        if (_sdf->HasElement("duration")) {
            this->duration = _sdf->Get<double>("duration");
        } else {
            gzerr << "[airlaunch_plugin] Please specify the force duration apply to the object.\n";
        }

        update_connection = event::Events::ConnectWorldUpdateBegin(
            std::bind(&AirLaunchPlugin::OnUpdate, this));

        node = transport::NodePtr(new transport::Node());
        node->Init(model->GetWorld()->Name());
        sub = node->Subscribe("~/launch", &AirLaunchPlugin::ReleaseCallback, this);
    }

    void OnUpdate() {

        if(isLaunched) {
            gazebo::common::Time gazebo_duration = model->GetWorld()->SimTime() - trigger_time;
            _duration = gazebo_duration.Double();
        }

        if (isLaunched && _duration < duration) {
            link->AddForce(force);
        }
    }

    void ReleaseCallback(ConstIntPtr &msg) {
        if (msg->data() == 1) {
            isLaunched = true;
            trigger_time = model->GetWorld()->SimTime();
            gzmsg << "Launched!\n";
        }
    }
};

GZ_REGISTER_MODEL_PLUGIN(AirLaunchPlugin)
}